


#define DEBUG 1
#define DHCP 1
#define HTTP 1


#ifdef DEBUG
#define DEBUG_PRINT(x)     Serial.print (x)
#define DEBUG_PRINTDEC(x)     Serial.print (x, DEC)
#define DEBUG_PRINTLN(x)  Serial.println (x)
#else
#define DEBUG_PRINT(x)
#define DEBUG_PRINTDEC(x)
#define DEBUG_PRINTLN(x)
#endif 

#include <SPI.h>
#include <Ethernet.h>

#include <Servo.h> 

// Enter a MAC address and IP address for your controller below.
// The IP address will be dependent on your local network:
byte mac[] = {  
  0x90, 0xA2, 0xDA, 0x00, 0x8B, 0xE7 };
IPAddress ip(192,168,0,12);

// initialize the library instance:
EthernetClient client;

EthernetServer server(80);

const int requestInterval = 30000;  // delay between requests. Avoid to break the limit of 150 requests/hour

char serverName[] = "api.twitter.com";  // twitter URL

boolean requested;                   // whether you've made a request since connecting
long lastAttemptTime = 0;            // last time you connected to the server, in milliseconds

String currentLine = "";            // string to hold the text from server
String tweet = "";                  // string to hold the tweet
String prevTweetId = "";                  // string to hold the tweet
boolean readingTweet = false;       // if you're currently reading the tweet
boolean readingError = false;
boolean readingId = false;


String httpQuery = "";

String serialLine = "";            // string to hold the text from serial
int incomingByte = 0; 

String command = "";

String verb = "";
String params[5];

int servoRightPosition = 0;
int servoLeftPosition = 0;
int servoHeadPosition = 0;

Servo servoRight;  // Right Arm 
Servo servoLeft; //Left Arm
Servo servoHead; //Head

int servoLeftPin = 22;
int servoHeadPin = 24;
int servoRightPin = 26;

int redLeftPin = 13; 
int greenLeftPin = 12; 
int blueLeftPin = 11; 

// Avoid pin 10 because of ethernet shield

int redRightPin = 4; 
int greenRightPin = 3;
int blueRightPin = 2;

int redHeadPin = 7;
int greenHeadPin = 6;
int blueHeadPin = 5;

int leftLed[] = {
  redLeftPin,
  greenLeftPin,
  blueLeftPin
};

int rightLed[] = {
  redRightPin,
  greenRightPin,
  blueRightPin
};

int headLed[] = {
  redHeadPin,
  greenHeadPin,
  blueHeadPin
};

int* leds[] = {
  leftLed,
  rightLed,
  headLed
};

void setup() {
  Serial.begin(9600);
  DEBUG_PRINTLN("Start..");

  initLed();

  initServo();

  verb.reserve(5);
  command.reserve(50);
#ifdef HTTP
  currentLine.reserve(256);
  tweet.reserve(140);
  prevTweetId.reserve(30);
  // attempt a DHCP connection:
#ifdef DHCP
  DEBUG_PRINTLN("DHCP configuration");
  if (Ethernet.begin(mac)) {
    DEBUG_PRINTLN("DHCP ok");
    allGreen(2000);
  }
  else {
    // if DHCP fails, start with a hard-coded address:
    DEBUG_PRINTLN("DHCP failed");
    Ethernet.begin(mac, ip);
  }
#else
  Ethernet.begin(mac, ip);
#endif

  // connect to Twitter:
  DEBUG_PRINTLN(Ethernet.localIP());
  server.begin();
  connectToServer();
#endif
}



void loop()
{
#ifdef HTTP
  doHttpServerJob();
  doHttpClientJob();
#endif
  doSerialClientJob();
}

void doSerialClientJob() {
  if (Serial.available() > 0) {
    // read the incoming byte:
    incomingByte = Serial.read();

    if (incomingByte == '\n') {
      Serial.println(serialLine);
      analyze(serialLine);
      serialLine = "";
    }
    else {
      serialLine += char(incomingByte);
    }
  }

}

#ifdef HTTP
void doHttpClientJob() {
  if (client.connected()) {
    if (client.available()) {
      // read incoming bytes:
      char inChar = client.read();
      // DEBUG_PRINT(inChar);

      // if you're currently reading the bytes of a tweet,

      // add them to the tweet String:
      if (readingTweet || readingId) {
        if (inChar != '<') {
          tweet += inChar;
        } 
        else {
          // if you got a "<" character,
          // you've reached the end of the tweet:

          if (readingError) {
            DEBUG_PRINTLN("ERROR:" + tweet);
            readingError = false;
          }
          if (readingId) {
            readingId = false;
            DEBUG_PRINTLN("id : " + tweet);
            if (prevTweetId.equals(tweet)) {
              client.stop();
            }
            else {
              prevTweetId = tweet;
            }
            //analyze(tweet);
          }
         
                    
          if (readingTweet) {
            readingTweet = false;
            DEBUG_PRINTLN("tweet : " + tweet);
            analyze(tweet);
           // close the connection to the server:
            client.stop();
        
          }
        }
      }

      // add incoming byte to end of line:
      currentLine += inChar; 

      // if you get a newline, clear the line:
      if (inChar == '\n') {
        currentLine = "";
      } 
      // if the current line ends with <text>, it will
      // be followed by the tweet:
      if ( currentLine.endsWith("<text>")) {
        // tweet is beginning. Clear the tweet string:
        readingTweet = true; 
        tweet = "";
      }
      if ( currentLine.endsWith("<error>")) {
          readingError = true; 
      }
      if ( currentLine.endsWith("<id>")) {
        // tweet is beginning. Clear the tweet string:
        readingId = true; 
        tweet = "";
      }
      /*
      if (currentLine.endsWith("<id>")) {
       readingId = true; 
       tweetId = "";
       }
       */
    }   
  }
  else if (millis() - lastAttemptTime > requestInterval) {
    connectToServer();
  }
}
#endif

#ifdef HTTP
void doHttpServerJob() {
  // listen for incoming clients
  EthernetClient incomingClient = server.available();
  if (incomingClient) {
    // an http request ends with a blank line
    boolean currentLineIsBlank = true;
    while (incomingClient.connected()) {
      if (incomingClient.available()) {
        char c = incomingClient.read();
        // if you've gotten to the end of the line (received a newline
        // character) and the line is blank, the http request has ended,
        // so you can send a reply
        if (c == '\n' && currentLineIsBlank) {
          // send a standard http response header
          incomingClient.println("HTTP/1.1 200 OK");
          incomingClient.println("Content-Type: text/html");
          incomingClient.println();

          // do the job
          incomingClient.println("Vous avez le bonjour de la marmotte");
          break;
        }
        if (c == '\n') {
          // you're starting a new line
          if (httpQuery.startsWith("GET /command?")) {
            httpQuery.replace("%20", " ");
            httpQuery.replace(" HTTP/1.1","");
            httpQuery.replace("GET /command?","");
            DEBUG_PRINT("Http command : ");
            DEBUG_PRINTLN(httpQuery);
            analyze(httpQuery);
          }

          httpQuery = "";
          currentLineIsBlank = true;
        } 
        else if (c != '\r') {
          // you've gotten a character on the current line
          httpQuery += c;
          currentLineIsBlank = false;
        }
      }
    }
    // give the web browser time to receive the data
    delay(1);
    // close the connection:
    incomingClient.stop();
  }
}

void connectToServer() {
  // attempt to connect, and wait a millisecond:
  DEBUG_PRINT("Memory : ");
  DEBUG_PRINTLN(memoryTest());

  DEBUG_PRINTLN("connecting to server...");
  if (client.connect(serverName, 80)) {
    DEBUG_PRINTLN("making HTTP request...");
    // make HTTP GET request to twitter:
    client.println("GET /1/statuses/user_timeline.xml?screen_name=SanityMarmot&count=1&trim_user=1 HTTP/1.1");
    client.println("HOST: api.twitter.com");
    client.println();
  }
  // note the time of this connect attempt:
  lastAttemptTime = millis();
}
#endif

void analyze(String commands) {
  commands.trim();
  DEBUG_PRINT("Analyze ");
  DEBUG_PRINTLN(commands);
  int commandEnd = commands.indexOf(';');
  if (commandEnd == -1) {
    commandEnd = commands.length();
  }

  command = commands.substring(0,commandEnd);
  execute(command);
  if (commandEnd + 1 < commands.length()) {
    analyze(commands.substring(commandEnd + 1));
  }
}

void execute(String command) {
  DEBUG_PRINT("Start command ");
  DEBUG_PRINTLN(command);

  command.trim();
  int verbEnd = command.indexOf(' ');
  if (verbEnd == -1) {
    verbEnd = command.length();
  }
  verb = command.substring(0, verbEnd);
  verb.toUpperCase();
  for(int i = 0; i < 5; i++) {
    params[i] = "";
  }
  int paramCount = 0;
  verbEnd++;
  int paramEnd = command.indexOf(' ', verbEnd);
  while(paramEnd != -1) {
    params[paramCount++] = command.substring(verbEnd, paramEnd);
    verbEnd = paramEnd;
    while ((command.charAt(verbEnd) == ' ')&&(verbEnd < command.length())) {
      verbEnd++;
    }
    paramEnd = command.indexOf(' ', verbEnd);
  }
  params[paramCount] = command.substring(verbEnd);

  DEBUG_PRINT("Verb :");
  DEBUG_PRINTLN(verb+".");
  for(int i = 0; i < 5; i++) {
    DEBUG_PRINT("Params["); 
    DEBUG_PRINT(i); 
    DEBUG_PRINT( "] :");
    DEBUG_PRINTLN(params[i]+".");
  }

  if (verb.equalsIgnoreCase("LAP")) {
    DEBUG_PRINTLN("DO LAP");
    int p = stringToInt(params[0]);
    setServoLeft(p);
    DEBUG_PRINTLN("LAP DONE");
  }

  if (verb.equalsIgnoreCase("RAP")) {
    DEBUG_PRINTLN("DO RAP");
    int p = stringToInt(params[0]);
    setServoRight(p);
    DEBUG_PRINTLN("RAP DONE");
  }

  if (verb.equalsIgnoreCase("HDP")) {
    DEBUG_PRINTLN("DO HDP");
    int p = stringToInt(params[0]);
    setServoHead(p);
    DEBUG_PRINTLN("HDP DONE");
  }

  if (verb.equalsIgnoreCase("CLAP")) {
    DEBUG_PRINTLN("DO CLAP");
    int p = stringToInt(params[0]);
    for (int i = 0; i < p; i++) {
      clap();
    }  
    DEBUG_PRINTLN("CLAP DONE");
  }

  if (verb.equalsIgnoreCase("LAC")) {
    DEBUG_PRINTLN("DOING LAC");
    analogWrite(redLeftPin, stringToInt(params[0]));
    analogWrite(greenLeftPin, stringToInt(params[1]));
    analogWrite(blueLeftPin, stringToInt(params[2]));
    DEBUG_PRINTLN("LAC DONE");
  }

  if (verb.equalsIgnoreCase("RAC")) {
    DEBUG_PRINTLN("DOING RAC");
    analogWrite(redRightPin, stringToInt(params[0]));
    analogWrite(greenRightPin, stringToInt(params[1]));
    analogWrite(blueRightPin, stringToInt(params[2]));
    DEBUG_PRINTLN("RAC DONE");
  }

  if (verb.equalsIgnoreCase("HDC")) {
    DEBUG_PRINTLN("DOING HDC");
    analogWrite(redHeadPin, stringToInt(params[0]));
    analogWrite(greenHeadPin, stringToInt(params[1]));
    analogWrite(blueHeadPin, stringToInt(params[2]));
    DEBUG_PRINTLN("HDC DONE");
  }

}

void clap() {
  int p = 0;
  setServoLeft(p);
  setServoRight(p);
  delay(500);
  p = 100;
  setServoLeft(p);
  setServoRight(p);
  delay(500);
}

void setServoRight(int p) {
  p = constrain(p, 0, 100);
  servoLeftPosition = p;
  int x = map(p, 0, 100, 20, 120);
  servoRight.write(x);
  DEBUG_PRINT("servoRightPosition :");
  DEBUG_PRINTLN(p);
}

void setServoLeft(int p) {
  p = constrain(p, 0, 100);
  servoLeftPosition = p;
  int x = map(p, 0, 100, 95, 0);
  servoLeft.write(x);
  DEBUG_PRINT("servoLeftPosition :");
  DEBUG_PRINTLN(p);
}

void setServoHead(int p) {
  p = constrain(p, 20, 100);
  servoHeadPosition = p;
  int x = map(p, 0, 100, 20, 120);
  servoHead.write(x);
  DEBUG_PRINT("servoHeadPosition :");
  DEBUG_PRINTLN(p);
}

int stringToInt(String param) {
  char p0[param.length() + 1];
  param.toCharArray(p0, sizeof(p0));
  int p = atoi(p0);
  return p;
}

// this function will return the number of bytes currently free in RAM
int memoryTest() {
  int byteCounter = 0; // initialize a counter
  byte *byteArray; // create a pointer to a byte array
  while ( (byteArray = (byte*) malloc (byteCounter * sizeof(byte))) != NULL ) {
    byteCounter++; // if allocation was successful, then up the count for the next try
    free(byteArray); // free memory after allocating it
  }

  free(byteArray); // also free memory after the function finishes
  return byteCounter; // send back the highest number of bytes successfully allocated
}

void initLed() {
  for (int i=0; i<3; i++) {
    for (int j=0; j<3; j++) {
        pinMode(leds[i][j], OUTPUT); 
    }
  }
  
  int tempo = 100;
  for (int j=0; j<3; j++) {
    for (int i=0; i<3; i++) {
      analogWrite(leds[i][j], 255);
      delay(tempo);
      analogWrite(leds[i][j], 0);
    }
  }
}

void allGreen(int d) {
      analogWrite(greenLeftPin, 255);
    analogWrite(greenRightPin, 255);
    analogWrite(greenHeadPin, 255);
    delay(d);
    analogWrite(greenLeftPin, 0);
    analogWrite(greenRightPin, 0);
    analogWrite(greenHeadPin, 0);
}

void initServo() {
  servoRight.attach(servoRightPin); 
  servoLeft.attach(servoLeftPin);
  servoHead.attach(servoHeadPin);

  setServoLeft(0);
  setServoRight(0);
  setServoHead(0);
}

